DMelt:DSP/6 Kalman Filter

Kalman filter

Read Kalman filter article first. The Kalman filter is implemented in the Java  jhplot.math.kalman.JKalman class.

Let us give an example of using the Kalman filter using the Python syntax.

from jhplot  import *
from java.util import Random
from jhplot.math.kalman import JKalman 
from jhplot.math.kalman.jama import Matrix

kal=JKalman(4,2)
rand =Random()

x,y=0,0
dx,dy=rand.nextDouble(),rand.nextDouble()
s =Matrix(4, 1)     #  state [x, y, dx, dy, dxy]        
c = Matrix(4, 1)    # corrected state [x, y, dx, dy, dxy]                

m = Matrix(2, 1)   #  measurement [x]
m.set(0, 0, x)
m.set(1, 0, y)

# transitions for x, y, dx, dy
tr = [ [1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1] ]
kal.setTransition_matrix(Matrix(tr))

# 1s somewhere?
kal.setError_cov_post(kal.getError_cov_post().identity())
print "first x:" + str(x) + ", y:" +str(y) + ", dx:" + str(dx) + ", dy:" + str(dy)

print "no; x; y; dx; dy; predictionX; predictionY; predictionDx; predictionDy; correctionX; correctionY; correctionDx; correctionDy;";
for i in range(200):
     s = kal.Predict()                 # check state before
     m.set(1, 0, rand.nextDouble())  # function init
     x = rand.nextGaussian()
     y = rand.nextGaussian()
     m.set(0, 0, m.get(0, 0) + dx + rand.nextGaussian());
     m.set(1, 0, m.get(1, 0) + dy + rand.nextGaussian());
     # a missing value (more then 1/4 times)
     if rand.nextGaussian() < -0.8:
             print "", i , ";;;;;", s.get(0, 0), ";", s.get(1, 0), ";" , s.get(2, 0) , ";" , s.get(3, 0), ";"
else:   #  measurement is ok : 
      c = kal.Correct(m) # look better
      print "" , i, ";" , m.get(0, 0), ";" , m.get(1, 0), ";", x, ";", y , ";", s.get(0, 0) ,";",s.get(1, 0), ";" ,s.get(2, 0) ,";",s.get(3, 0), ";",c.get(0, 0), ";", c.get(1, 0), ";", c.get(2, 0), ";" ,c.get(3, 0),";"